
#include <algorithm>

#include "Rect.h"
#include "Vector2.h"
#include "Circle.h"
#include <stdlib.h>
#include <list>
#include "Mathematics.h"
#include "RenderingManager.h"

namespace Math
{

//used to instantiate static vectors for each type.
template<> Vector2Templ<int> Vector2Templ<int>::Zero = Vector2Templ<int>(0,0);
template<> Vector2Templ<float> Vector2Templ<float>::Zero = Vector2Templ<float>(0,0);
template<> Vector2Templ<double> Vector2Templ<double>::Zero = Vector2Templ<double>(0,0);

template<> Vector2Templ<int> Vector2Templ<int>::One = Vector2Templ<int>(1,1);
template<> Vector2Templ<float> Vector2Templ<float>::One = Vector2Templ<float>(1,1);
template<> Vector2Templ<double> Vector2Templ<double>::One = Vector2Templ<double>(1,1);

template<> Vector2Templ<int> Vector2Templ<int>::Up = Vector2Templ<int>(0,-1);
template<> Vector2Templ<float> Vector2Templ<float>::Up = Vector2Templ<float>(0,-1);
template<> Vector2Templ<double> Vector2Templ<double>::Up = Vector2Templ<double>(0,-1);

template<> Vector2Templ<int> Vector2Templ<int>::Down = Vector2Templ<int>(0,1);
template<> Vector2Templ<float> Vector2Templ<float>::Down = Vector2Templ<float>(0,1);
template<> Vector2Templ<double> Vector2Templ<double>::Down = Vector2Templ<double>(0,1);

template<> Vector2Templ<int> Vector2Templ<int>::Left = Vector2Templ<int>(-1,0);
template<> Vector2Templ<float> Vector2Templ<float>::Left = Vector2Templ<float>(-1,0);
template<> Vector2Templ<double> Vector2Templ<double>::Left = Vector2Templ<double>(-1,0);

template<> Vector2Templ<int> Vector2Templ<int>::Right = Vector2Templ<int>(1,0);
template<> Vector2Templ<float> Vector2Templ<float>::Right = Vector2Templ<float>(1,0);
template<> Vector2Templ<double> Vector2Templ<double>::Right = Vector2Templ<double>(1,0);

/**
 * Finds the two points of contact of two rects.
 * @param rect1
 * @param rect2
 * @param points An array of Vector2I[2]
 * @return
 */
int CommonPoints(const Rect& rect1, const Rect& rect2, Vector2I commonPoints[])
{
    //create a list of the points of the second rect
    Vector2I points[] = {
        Vector2I(rect2.GetLeft(), rect2.GetTop()),
        Vector2I(rect2.GetTotalWidth(), rect2.GetTop()),
        Vector2I(rect2.GetTotalWidth(), rect2.GetTotalHeight()),
        Vector2I(rect2.GetLeft(), rect2.GetTotalHeight())
    };
    //a list to store for each point the dif between it and its opposite
    //for each point of second rect tha intersects with the first
    int intersectingList[4];
    int pointsC = 0;
    //for 0 , 1 points
    for (int i = 0; i < 4; i++)
    {
        if (rect1.Intersect(points[i]))
        {
            intersectingList[pointsC] = i;
            pointsC++;
        }
    }

    //make the appropriate action based on the number of intersecting points
    if (pointsC == 0)
    {
        return 0;
    }
    else if (pointsC == 1)
    {
        int index = intersectingList[0];
        Vector2I vec = (index < 2) ? points[index] - points[index + 2] : points[index] - points[index - 2];

        int x2 = (vec.GetX() < 0) ? rect1.GetTotalWidth() : rect1.GetLeft();
        int y1 = (vec.GetY() < 0) ? rect1.GetTotalHeight() : rect1.GetTop();

        commonPoints[0].Set(points[index].GetX(), y1);
        commonPoints[1].Set(x2, points[index].GetY());
        return 1;

    }
    else if (pointsC == 2)
    {
        int index1 = intersectingList[0];
        int index2 = intersectingList[1];

        Vector2I p1 = (index1 < 2) ? points[index1] - points[index1 + 2] : points[index1] - points[index1 - 2];
        Vector2I p2 = (index2 < 2) ? points[index2] - points[index2 + 2] : points[index2] - points[index2 - 2];

        Vector2I normal = p1 - p2;

        normal.Normalize();
        //normal.Set(normal.GetY(), normal.GetX());
        if (normal.GetX() == 0)
        {
            int x = (normal.GetY() > 0) ? rect1.GetLeft() : rect1.GetTotalWidth();
            commonPoints[0].Set(x, points[index1].GetY());
            commonPoints[1].Set(x, points[index2].GetY());
        }
        else
        {
            int y = (normal.GetX() > 0) ? rect1.GetTop() : rect1.GetTotalHeight();
            commonPoints[0].Set(points[index1].GetX(), y);
            commonPoints[1].Set(points[index2].GetX(), y);
        }
        return 2;

    }
    else if (pointsC == 4)
    {
        return 4;
    }

    return 0;
}

/**
 * Return strue if both numbers are negative or positive.
 */
bool SameSign(float a,float b)
{
    return ((a >0 && b >0)||(a <0 && b <0));
}

/**
 * Return the float number whose absolute value is the least.
 */
float AbsoluteMin(float a, float b)
{
    if (std::abs(a) < std::abs(b))
    {
        return a;
    }
    else
        return b;
}

/**
 * Return the float number whose absolute value is the least.
 */
float AbsoluteMax(float a, float b)
{
    if (std::abs(a) > std::abs(b))
    {
        return a;
    }
    else
        return b;
}

/**
 * Return true if thea absolute value of a is lesser than the absolute value of b.
 */
bool IsAbsoluteMax(float a, float b)
{
    return (std::abs(a) > std::abs(b));
}

/**
 * Return true if thea absolute value of a is lesser than the absolute value of b.
 */
bool IsAbsoluteMin(float a, float b)
{
    return (std::abs(a) < std::abs(b));
}

float RadiansToDegrees(float rad)
{
    return static_cast<float> (rad * (180.0 / PI));
}

float DegreesToRadians(float deg)
{
    return static_cast<float> (deg * (PI / 180.0f));
}

/*Generic functions which return the distance between the centers of two objects*/

Vector2Templ<float> DistanceCenter(const Rect& rect1, const Rect& rect2)
{
    return rect1.GetCenter() - rect2.GetCenter();
}

Vector2Templ<float> DistanceCenter(const Circle& circle1, const Circle& circle2)
{
    return circle1.GetCenter() - circle2.GetCenter();
}

Vector2Templ<float> DistanceCenter(const Circle& circle, const Rect& rect)
{
    return circle.GetCenter() - rect.GetCenter();
}

Vector2Templ<float> DistanceCenter(const Circle& circle, const Vector2F& vector)
{
    return circle.GetCenter() - vector;
}

Vector2Templ<float> DistanceCenter(const Rect& rect, const Vector2F& vector)
{
    return rect.GetCenter() - vector;
}

/*Generic functions which return the distance between two objects*/

Vector2Templ<float> DistanceClosestPoint(const Rect& rect1, const Rect& rect2)
{
    int xDst;
    {//calculate the x component of the distance vector
        //initialize the y dimension elements
        int startX1 = rect1.GetLeft();
        int startX2 = rect2.GetLeft();
        int endX1 = rect1.GetTotalWidth();
        int endX2 = rect2.GetTotalWidth();

        if (startX1 > endX2)
            xDst = endX2 - startX1;
        else if (startX2 > endX1)
            xDst = endX1 - startX2;
        else //if there is a collision on x axis the x component must be null
            xDst = 0;
    }
    int yDst;
    {//calculate the y component of the distance vector
        //initialize the y dimension elements
        int startY1 = rect1.GetTop();
        int startY2 = rect2.GetTop();
        int endY1 = rect1.GetTotalHeight();
        int endY2 = rect2.GetTotalHeight();

        if (startY1 > endY2)
            yDst = endY2 - startY1;
        else if (startY2 > endY1)
            yDst = endY1 - startY2;
        else //if there is a collision on y axis the y component must be null
            yDst = 0;
    }
    return Vector2Templ<float>(xDst, yDst);
}

Vector2Templ<float> DistanceClosestPoint(const Circle& circle, const Vector2Templ<float>& vector)
{
    Vector2F dist = DistanceCenter(circle, vector);
    return dist - dist.Normal() * circle.GetRadious();
}

Vector2Templ<float> DistanceClosestPoint(const Rect& rect1, const Vector2Templ<float>& vector)
{
    int xDst;
    {//calculate the x component of the distance vector
        //initialize the y dimension elements
        int startX1 = rect1.GetLeft();
        int startX2 = vector.GetX();
        int endX1 = rect1.GetTotalWidth();

        if (startX1 < startX2)
            xDst = startX2 - startX1;
        else if (endX1 < startX2)
            xDst = startX2 - endX1;
        else
            xDst = 0;
    }
    int yDst;
    {//calculate the y component of the distance vector
        //initialize the y dimension elements
        int startY1 = rect1.GetTop();
        int startY2 = vector.GetY();
        int endY1 = rect1.GetTotalHeight();

        if (startY1 < startY2)
            yDst = startY2 - startY1;
        else if (endY1 < startY2)
            yDst = startY2 - endY1;
        else
            yDst = 0;
    }
    return Vector2Templ<float>(xDst, yDst);
}

Vector2Templ<float> DistanceClosestPoint(const Circle& circle1, const Circle& circle2)
{
    Vector2F dist = DistanceCenter(circle1, circle2);
    return dist - dist.Normal() * (circle1.GetRadious() + circle2.GetRadious());
}

Vector2Templ<float> DistanceClosestPoint(const Circle& circle, const Rect& rect)
{
    /*for more info see Intersect(const Circle& circle, const Rect& rect)*/
    int cTop = static_cast<int> (circle.GetCenter().GetY() - circle.GetRadious());
    int cLeft = static_cast<int> (circle.GetCenter().GetX() - circle.GetRadious());
    {
        int cBottom = static_cast<int> (circle.GetCenter().GetY() + circle.GetRadious());
        int cRight = static_cast<int> (circle.GetCenter().GetX() + circle.GetRadious());

        Vector2F dPoint(0, 0);
        //check if it is valid to consider the circle as a rect
        if (cBottom <= rect.GetTop())
            dPoint.SetY(rect.GetTop());
        else if (cTop >= rect.GetTotalHeight())
            dPoint.SetY(rect.GetTotalHeight());

        if (dPoint.GetX() != 0)
        {
            if (cRight <= rect.GetLeft())
                dPoint.SetX(rect.GetLeft());
            else if (cLeft >= rect.GetTotalWidth())
                dPoint.SetX(rect.GetTotalWidth());

            if (!dPoint.GetY() != 0)
                return dPoint;
        }
    }
    {
        Rect boundingBox(cTop, cLeft, static_cast<int> (circle.GetRadious()*2), static_cast<int> (circle.GetRadious()*2));
        return rect.DistanceClosestPoint(boundingBox);
    }
}

/*Basic functions which return true if two object intersect with each other*/

/*All intersections should return false on touch*/

bool Intersect(const Rect& rect1, const Rect& rect2)
{
    //check if all projections on the two axis collide
    if (rect1.GetLeft() >= rect2.GetTotalWidth()) return false;
    if (rect2.GetLeft() >= rect1.GetTotalWidth()) return false;
    if (rect1.GetTop() >= rect2.GetTotalHeight()) return false;
    if (rect2.GetTop() >= rect1.GetTotalHeight()) return false;
    return true;
}

bool Intersect(const Rect& rect, const Vector2I& vec)
{
    if (vec.GetX() < rect.GetLeft() || vec.GetX() > rect.GetTotalWidth()) return false;
    if (vec.GetY() < rect.GetTop() || vec.GetY() > rect.GetTotalHeight()) return false;
    return true;
}

bool Intersect(const Rect& rect, const Vector2F& vec)
{
    if (vec.GetX() < rect.GetLeft() || vec.GetX() > rect.GetTotalWidth()) return false;
    if (vec.GetY() < rect.GetTop() || vec.GetY() > rect.GetTotalHeight()) return false;
    return true;
}

bool Intersect(const Circle& circle, const Vector2I& vec)
{
    //the sqr distance is used for performance reasons
    double radious = circle.GetRadious();
    //we check if the point is inside circles radious
    return (circle.GetCenter().DistanceSqr(vec) <= radious * radious);
}

/*          |                 |
 *     A    |      B          |   C
 *  ------------------------------------
 *          |*****************|
 *     D    |*     RECT      *|   E
 *          |*****************|
 *  -------------------------------------
 *     F    |       G         |   J
 *          |                 |
 */

/* If the circle is loacated on A , C , F or J the it is unaccurate to
 * use its bounding box in order to calculate intersection.
 * In that case we must calculate in which of the for loactions it is
 * and then ( by finding which rect point it is looking at ) calculate the
 * close point distance between the two shapes. If the distance is greater
 * than the radious of the circle then we do not have a collision.
 * */

bool Intersect(const Circle& circle, const Rect& rect)
{
    float cTop = circle.GetCenter().GetY() - circle.GetRadious();
    float cLeft = circle.GetCenter().GetX() - circle.GetRadious();
    float cBottom = static_cast<float> (circle.GetCenter().GetY() + circle.GetRadious());
    float cRight = static_cast<float> (circle.GetCenter().GetX() + circle.GetRadious());
    {
        //find out if circle is located in A C F or J
        //dPoint will be the closest point towards circle
        Vector2I dPoint(0, 0);

        if (cBottom < rect.GetTop())
            dPoint.SetY(rect.GetTop());
        else if (cTop > rect.GetTotalHeight())
            dPoint.SetY(rect.GetTotalHeight());

        if (dPoint.GetX() != 0)
        {
            if (cRight < rect.GetLeft())
                dPoint.SetX(rect.GetLeft());
            else if (cLeft > rect.GetTotalWidth())
                dPoint.SetX(rect.GetTotalWidth());

            if (!dPoint.GetY() != 0)
                return (dPoint.Distance(circle.GetCenter()) < circle.GetRadious());
        }
    }
    //it is safe to calculate intersection with the bounding box of the circle
    //instead of creating a temp rect we make direct calculation
    //this is done due to percision reasons (rect holds only int values and
    //cannot deliver the precission of radious )
    {
        //check if all projections on the two axis collide
        if (rect.GetLeft() >= cRight) return false;
        if (cLeft >= rect.GetTotalWidth()) return false;
        if (rect.GetTop() >= cBottom) return false;
        if (cTop >= rect.GetTotalHeight()) return false;
        return true;
    }
}

bool Intersect(const Circle& circle1, const Circle& circle2)
{
    return (circle1.GetCenter().Distance(circle2.GetCenter()) < circle1.GetRadious() + circle2.GetRadious());
}

IntersectionType Intersect(const Rect& rect1, const Rect& rect2, Vector2Templ<float>& axisInfo)
{
    int diffx;
    if (!OneAxisIntersection(rect1.GetLeft(), rect1.GetTotalWidth(), rect2.GetLeft(), rect2.GetTotalWidth(), &diffx))
        return INTER_NO;

    int diffy;
    if (!OneAxisIntersection(rect1.GetTop(), rect1.GetTotalHeight(), rect2.GetTop(), rect2.GetTotalHeight(), &diffy))
        return INTER_NO;

    axisInfo.Set(diffx, diffy);

    if (diffx == 0 || diffy == 0)
        return INTER_TOUCH;
    else
        return INTER_FULL;
}

bool OneAxisIntersection(int startA, int endA, int startB, int endB, int* diff)
{
    int diff1 = endB - startA;
    int diff2 = startB - endA;
    if (std::abs(diff1) < std::abs(diff2))
    {
        *diff = diff1;
        return (diff1 >= 0);
    }
    *diff = diff2;
    return (diff2 <= 0);
}

IntersectionType Intersect(const Circle& circle, const Rect& rect, Vector2Templ<float>& axisInfo)
{
    float cTop = circle.GetCenter().GetY() - circle.GetRadious();
    float cLeft = circle.GetCenter().GetX() - circle.GetRadious();
    float cBottom = static_cast<float> (circle.GetCenter().GetY() + circle.GetRadious());
    float cRight = static_cast<float> (circle.GetCenter().GetX() + circle.GetRadious());
    // <editor-fold defaultstate="collapsed" desc="First Part of method">
    {
        //find out if circle is located in A C F or J
        //dPoint will be the closest point towards circle
        Vector2F dPoint(0, 0);

        if (cBottom <= rect.GetTop())
            dPoint.SetY(rect.GetTop());
        else if (cTop >= rect.GetTotalHeight())
            dPoint.SetY(rect.GetTotalHeight());

        if (dPoint.GetX() != 0)
        {
            if (cRight <= rect.GetLeft())
                dPoint.SetX(rect.GetLeft());
            else if (cLeft >= rect.GetTotalWidth())
                dPoint.SetX(rect.GetTotalWidth());

            if (!dPoint.GetY() != 0)
            {
                if (std::abs(dPoint.Distance(circle.GetCenter()) - circle.GetRadious()) <= PRECISION)
                {
                    axisInfo.Set(0, 0);
                    return INTER_TOUCH;
                }
                else if (dPoint.Distance(circle.GetCenter()) < circle.GetRadious())
                {
                    //a vector from circle's center towards dpoint
                    Vector2F diff(dPoint - circle.GetCenter());
                    //represent x axis with a vector
                    Vector2F unit(1, 0);
                    //the angle between x axis and diff vector
                    float thita = diff.AngleInRads(unit);
                    //a vector from the circle's center towards its paremeter
                    //especially to the point where radious and x axis are
                    //parellel
                    Vector2F rad(cRight, static_cast<float> (circle.GetCenter().GetY()));
                    //rotate the rad vector to face where diff faces
                    rad.RotateVector(thita);
                    //get the vector between dpoint and the circle inside the rect
                    axisInfo = (rad - diff);
                    return INTER_FULL;
                }
            }
        }
    }// </editor-fold>

    //it is safe to calculate intersection with the bounding box of the circle
    //instead of creating a temp rect we make direct calculation
    //this is done due to percision reasons (rect holds only int values and
    //cannot deliver the precission of radious )
    {
        bool touch = false;

        // <editor-fold defaultstate="collapsed" desc="X axis checking">
        {
            int L1 = rect.GetLeft();
            int W1 = rect.GetTotalWidth();

            if ((W1 > cLeft) && (cLeft > L1))
            {
                //x axis collision
                //set the x of axis info to the shortest distance between
                //the two shapes
                if (std::abs(L1 - cLeft) > std::abs(cLeft - W1))
                    axisInfo.SetX(cLeft - W1);
                else
                    axisInfo.SetX(L1 - cLeft);
            }
            else if ((cRight > L1) && (L1 > cLeft))
            {
                //x axis collision
                if (std::abs(cLeft - L1) > std::abs(L1 - cRight))
                    axisInfo.SetX(L1 - cRight);
                else
                    axisInfo.SetX(cLeft - L1);
            }
            else if ((W1 == cLeft) || (cRight == L1) || (L1 == cLeft))
            {
                //x axis touch
                axisInfo.SetX(0);
                touch = true;
            }
            else
            {
                //no collision
                axisInfo.Set(0, 0);
                return INTER_NO;
            }
        }// </editor-fold>

        // <editor-fold defaultstate="collapsed" desc="Y axis checking">
        {
            int T1 = rect.GetTop();
            int H1 = rect.GetTotalHeight();

            if ((H1 > cTop) && (cTop > T1))
            {
                //y axis collision
                //set the y of axis info to the shortest distance between
                //the two shapes
                if (std::abs(T1 - cTop) > std::abs(cTop - H1))
                    axisInfo.SetY(cTop - H1);
                else
                    axisInfo.SetY(T1 - cTop);
            }
            else if ((cBottom > T1) && (T1 > cTop))
            {
                //y axis collision
                if (std::abs(cTop - T1) > std::abs(T1 - cBottom))
                    axisInfo.SetY(T1 - cBottom);
                else
                    axisInfo.SetY(cTop - T1);
            }
            else if ((cTop == H1) || (cBottom == T1) || (cTop == T1))
            {
                //y axis touch
                axisInfo.SetY(0);
                touch = true;
            }
            else
            {
                //no collision
                axisInfo.Set(0, 0);
                return INTER_NO;
            }
        }// </editor-fold>

        if (touch)
            return INTER_TOUCH;
        return INTER_FULL;
    }
}

IntersectionType Intersect(const Circle& circle1, const Circle& circle2, Vector2Templ<float>& axisInfo)
{
    double distance = circle1.DistanceCenter(circle2);
    double compRadious = circle1.GetRadious() + circle2.GetRadious();
    /* if distnce - compRadious is almost equal return touch.*/
    double diff = (compRadious - distance);
    if (eq(diff, 0.0f))
    {
        axisInfo.Set(0, 0);
        return INTER_TOUCH;
    }
    else if (diff > 0)
    {
        //a vector from the first circle's center towards the center of the other
        Vector2F diffVector((circle1.GetCenter() - circle2.GetCenter()).ConvertTo<float>());
        //represent x axis with a vector
        Vector2F unit(1, 0);
        //the angle between x axis and diff vector
        float thita = diffVector.AngleInRads(unit);
        //initialize rad as a unit vector parellel to x axis
        Vector2F rad(1, 0);
        //rotate the rad vector to face where diff faces
        rad.RotateVector(thita);
        //multiply rad with diff to correct its length
        rad *= diff;
        //get the vector between dpoint and the circle inside the rect
        axisInfo = rad;
        return INTER_FULL;
    }
    return INTER_NO;
}

}
